#include "ti_msp_dl_config.h"
#include "Variables.h"

uint32_t test_num;          //测试值
int32_t Left_speed;         //左轮速度
int32_t Right_speed;        //右轮速度

int32_t Left_speed_pid;     //左轮速度环的值
int32_t Right_speed_pid;    //右轮速度环的值
int32_t Left_Angle_pid;     //左轮角度环的值
int32_t Right_Angle_pid;    //右轮角度环的值
int32_t Left_Distance_pid;     //左轮位移环的值
int32_t Right_Distance_pid;    //右轮位移环的值
int32_t Left_Gray_pid;          //左轮灰度环的值
int32_t Right_Gray_pid;         //右轮灰度环的值

int32_t last_left_speed_pid;    //左轮上次的速度值
int32_t last_right_speed_pid;   //右轮上次的速度值

uint8_t Curr_State = 0x00;      //状态
uint8_t Last_State = 0x55;      //状态

uint8_t Filter_Cnt=0;           //用于滤波的计数
uint8_t Delay_Cnt=0;           //用于延时的计数
uint8_t Circle_Cnt=0;           //用于第四问圈数的判断
uint8_t flag_Circle=0;           //用于第四问圈数的判断

uint8_t Tishi_flag=0;           //声光提示
uint8_t Error_less_flag=0;           //误差减小flag
uint8_t FMQ_flag=1;

uint16_t Speed_goal=110;
void TIMER_0_INST_IRQHandler(void)
{
    switch (DL_TimerG_getPendingInterrupt(TIMER_0_INST)) {
        //关注Zero Event事件
        case DL_TIMER_IIDX_ZERO:
//-----------------------计算速度  begin----------------------------//
        Left_speed=Encoder_Count_Left_temp;
        Right_speed=Encoder_Count_Right_temp;
        Encoder_Count_Left_temp=0;
        Encoder_Count_Right_temp=0;
//-----------------------TEST   begin----------------------------//
        test_num++;
        // Gray_Get_Error();                                                       //获取灰度误差
        // Left_speed_pid=Speed_Left_control(&Speed_Left,Left_speed,60);           //速度环
        // Right_speed_pid=Speed_Right_control(&Speed_Right,Right_speed,60);       //速度环
        // Left_Gray_pid=Gray_control(&Gray,Gray_Error,0);                         //灰度环
        // Right_Gray_pid=-Left_Gray_pid;                                          //灰度环
        // set_Motor(Left_Gray_pid,Right_Gray_pid); //赋值
        // // set_Motor(Left_speed_pid,Right_speed_pid); //赋值
//-----------------------第一问    begin------------------------------//
        if(cmd=='A')
        {   
            switch (Curr_State) 
            {
                case 0x00:
                    Last_State=Curr_State;
                    Left_speed_pid=Speed_Left_control(&Speed_Left,Left_speed,80);       
                    Right_speed_pid=Speed_Right_control(&Speed_Right,Right_speed,80);
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, 0);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_speed_pid+Left_Angle_pid,Right_speed_pid+Right_Angle_pid);
                    if (Read_Huidu()) 
                    {
                        if (++Filter_Cnt >= 2) 
                        {
                            Curr_State = 0x01;
                            Filter_Cnt = 0; // 防止溢出
                        }
                    }
                break;

                case 0x01:
                    if (Last_State==0x00&&Curr_State==0x01) 
                    {
                        Tishi_flag=1;
                    }
                    Last_State=Curr_State;
                    set_Motor(0,0);
                break;

                default:
                    Curr_State=0x00;
                break;
            }
            
        }
//-----------------------第二问    begin------------------------------//
        else if(cmd=='B')
        {
            switch (Curr_State) 
            {
                case 0x00://直道
                    Last_State=Curr_State;
                    Left_speed_pid=Speed_Left_control(&Speed_Left,Left_speed,80);       
                    Right_speed_pid=Speed_Right_control(&Speed_Right,Right_speed,80);
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, 0);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_speed_pid+Left_Angle_pid,Right_speed_pid+Right_Angle_pid);
                    if (Read_Huidu()) 
                    {
                        if (++Filter_Cnt >= 1) 
                        {
                            Curr_State = 0x01;
                            Filter_Cnt = 0; // 防止溢出
                        }
                    }
                break;

                case 0x01://弯道
                    if (Last_State==0x00&&Curr_State==0x01) 
                    {
                        Tishi_flag=1;
                    }
                    Last_State=Curr_State;
                    Angle.integral=0;                                                       //角度环积分清零
                    Gray_Get_Error();                                                       //获取灰度误差
                    Left_speed_pid=Speed_Left_control(&Speed_Left,Left_speed,100);           //速度环
                    Right_speed_pid=Speed_Right_control(&Speed_Right,Right_speed,100);       //速度环
                    Left_Gray_pid=Gray_control(&Gray,Gray_Error,0);                         //灰度环
                    Right_Gray_pid=-Left_Gray_pid;                                          //灰度环
                    set_Motor(Left_speed_pid+Left_Gray_pid,Right_speed_pid+Right_Gray_pid); //赋值
                    if (!Read_Huidu()) 
                    {
                        Curr_State = 0x02;
                    }
                break;

                case 0x02://直道
                    if (Last_State==0x01&&Curr_State==0x02) 
                    {
                        Tishi_flag=1;
                    }
                    Last_State=Curr_State;
                    Left_speed_pid=Speed_Left_control(&Speed_Left,Left_speed,80);       
                    Right_speed_pid=Speed_Right_control(&Speed_Right,Right_speed,80);
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, 180);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_speed_pid+Left_Angle_pid,Right_speed_pid+Right_Angle_pid);
                    if (Read_Huidu()) 
                    {
                        Curr_State = 0x03;
                    }
                break;

                case 0x03://弯道
                    if (Last_State==0x02&&Curr_State==0x03) 
                    {
                        Tishi_flag=1;
                    }
                    Last_State=Curr_State;
                    Gray_Get_Error();                                                       //获取灰度误差
                    Left_speed_pid=Speed_Left_control(&Speed_Left,Left_speed,80);           //速度环
                    Right_speed_pid=Speed_Right_control(&Speed_Right,Right_speed,80);       //速度环
                    Left_Gray_pid=Gray_control(&Gray,Gray_Error,0);                         //灰度环
                    Right_Gray_pid=-Left_Gray_pid;                                          //灰度环
                    set_Motor(Left_speed_pid+Left_Gray_pid,Right_speed_pid+Right_Gray_pid); //赋值
                    if (!Read_Huidu()) 
                    {
                        if (++Filter_Cnt >= 2) 
                        {
                            Curr_State = 0x04;
                            Filter_Cnt = 0; // 防止溢出
                        }
                    }
                break;

                case 0x04://停车
                    if (Last_State==0x03&&Curr_State==0x04) 
                    {
                        Tishi_flag=1;
                    }
                    Last_State=Curr_State;
                    set_Motor(0,0);
                break;
                default:
                    Curr_State=0x00;
                break;
            }
        }
//-----------------------第三问    begin------------------------------//
        else if(cmd=='C')
        {
            if (Last_State == 0x55) 
            {
                PID_Init(&Distance_Left,1,0.001,1.5,500,1000);  //左轮位移环PID初始化
                PID_Init(&Distance_Right,1,0.001,1.5,500,1000); //右轮位移环PID初始化
                PID_Init(&Xiexian_Angle,-3,-0.2,-13,500,1000); //斜线角度PID初始化
                PID_Init(&Speed_Left,10,2,0.5,500,1000);        //左轮速度环PID初始化
                PID_Init(&Speed_Right,10,2,0.5,500,1000);       //右轮速度环PID初始化
                PID_Init(&Angle,-14,-0.2,-35,500,1000);          //第四问角度环PID初始化, 14,0.2
            }
            
            switch (Curr_State) 
            {
                case 0x00://左上原地转弯
                    if (Last_State==0x07&&Curr_State==0x00) 
                    {
                        Tishi_flag=1;
                    }
                    Last_State=Curr_State;
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -41);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Angle_pid,Right_Angle_pid);
                    if (test_num>=20) 
                    {
                        Curr_State=0x01;
                        test_num=0;
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0x01://左上到右下斜道
                    if (Last_State==0x00&&Curr_State==0x01) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        flag_stop=0;
                        FMQ_flag=1;
                    }
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }
                    Last_State=Curr_State;
                    Left_Distance_pid=Distance_Left_control(&Distance_Left,Encoder_Count_Left,4100);        //距离环
                    Right_Distance_pid=Distance_Right_control(&Distance_Right,Encoder_Count_Right,4100);
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -41);                                   //角度环
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Distance_pid+Left_Angle_pid,Right_Distance_pid+Right_Angle_pid); 
                    if (test_num>=35) 
                    {
                        Curr_State=0x02;
                        test_num=0;
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0x02://右下原地转弯
                    if (Last_State==0x01&&Curr_State==0x02) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, 0);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Angle_pid,Right_Angle_pid);
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }
                    if (test_num>=20) 
                    {
                        Curr_State=0xAA;
                        test_num=0;
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0xAA://右下原地转弯后前走
                    if (!Read_Huidu()) 
                    {
                        set_Motor(300, 350);
                    }
                    else if(Read_Huidu())
                    {
                        Curr_State=0x03;
                        if (FMQ_flag==1) 
                        {
                            Tishi_flag=1;
                        }
                    }
                break;

                case 0x03://右边弯道
                    if ((Last_State==0x02||Last_State==0x01)&&Curr_State==0x03) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        Error_less_flag=1;
                    }
                    Last_State=Curr_State;
                    Gray_Get_Error();                                                       //获取灰度误差
                    Left_speed_pid=Speed_Left_control(&Speed_Left,Left_speed,90);           //速度环
                    Right_speed_pid=Speed_Right_control(&Speed_Right,Right_speed,90);       //速度环
                    Left_Gray_pid=Gray_control(&Gray,Gray_Error,0);                         //灰度环
                    Right_Gray_pid=-Left_Gray_pid;                                          //灰度环
                    set_Motor(Left_speed_pid+Left_Gray_pid,Right_speed_pid+Right_Gray_pid); //赋值
                    
                    if (!Read_Huidu()) 
                    {
                        
                        Filter_Cnt = 0; // 防止溢出
                        Curr_State = 0xFF;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        Tishi_flag=1;
                        
                    }
                break;

                case 0xFF://右上角出弯延时
                    if (Last_State==0x03&&Curr_State==0xFF) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    set_Motor(0,0);
                    if (test_num>=5) 
                    {
                        Curr_State = 0x04;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0x04://右上角原地转弯
                    if (Last_State==0xFF&&Curr_State==0x04) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -140);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Angle_pid,Right_Angle_pid);
                    if (test_num>=20) 
                    {
                        Curr_State=0x05;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分

                    }
                break;

                case 0x05://右上到左下斜道
                    if (Last_State==0x04&&Curr_State==0x05) 
                    {
                        flag_stop=0;
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        FMQ_flag=1;
                    }
                    Last_State=Curr_State;
                    Left_Distance_pid=Distance_Left_control(&Distance_Left,Encoder_Count_Left,4100);        //距离环
                    Right_Distance_pid=Distance_Right_control(&Distance_Right,Encoder_Count_Right,4100);
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -140);                                   //角度环
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Distance_pid+Left_Angle_pid,Right_Distance_pid+Right_Angle_pid); 
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }
                    if (test_num>=35) 
                    {
                        Curr_State=0x06;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;
                
                case 0x06://左下原地转弯
                    if (Last_State==0x05&&Curr_State==0x06) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -180);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Angle_pid,Right_Angle_pid);
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }
                    if (test_num>=20) 
                    {
                        Curr_State=0xBB;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0xBB://左下原地转弯后前走
                    if (!Read_Huidu()) 
                    {
                        set_Motor(450, 300);
                    }
                    else if(Read_Huidu())
                    {
                        Curr_State=0x07;
                        if (FMQ_flag==1) 
                        {
                            Tishi_flag=1;
                        }
                        
                    }
                break;

                case 0x07://左边弯道
                    if ((Last_State==0x06||Last_State==0x05)&&Curr_State==0x07) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        Error_less_flag=1;          //误差减小
                        flag_Circle=0x06;//用于圈数计算
                    }
                    Last_State=Curr_State;
                    if (flag_Circle==0x06 && Curr_State==0x07) 
                    {
                        flag_Circle=0x07;
                        Circle_Cnt++;  
                    }
                    Gray_Get_Error();                                                       //获取灰度误差
                    Left_speed_pid=Speed_Left_control(&Speed_Left,Left_speed,90);           //速度环
                    Right_speed_pid=Speed_Right_control(&Speed_Right,Right_speed,90);       //速度环
                    Left_Gray_pid=Gray_control(&Gray,Gray_Error,0);                         //灰度环
                    Right_Gray_pid=-Left_Gray_pid;                                          //灰度环
                    set_Motor(Left_speed_pid+Left_Gray_pid,Right_speed_pid+Right_Gray_pid); //赋值 
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }                   
                    if (!Read_Huidu()) 
                    {
                        Tishi_flag=1;
                        Curr_State = 0x08;
                    }
                    else if(!Read_Huidu())
                    {
                        Curr_State = 0xFE;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        Tishi_flag=1;
                    }
                break;

                case 0xFE://左上出弯延时
                    if (Last_State==0x07&&Curr_State==0xFE) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    set_Motor(0,0);
                    if (test_num>=5) 
                    {
                        Curr_State = 0x00;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0x08://停车
                    set_Motor(0,0);    
                    Last_State=Curr_State;
                break;

                default:
                    Curr_State=0x00;
                    Last_State=Curr_State;
                break;
            }
        }

//--------------------------------------------------第四问    begin--------------------------------------------------------------//
        else if(cmd=='D')
        {
            if (Last_State == 0x55) 
            {
                PID_Init(&Distance_Left,1,0.001,1.5,500,1000);  //左轮位移环PID初始化
                PID_Init(&Distance_Right,1,0.001,1.5,500,1000); //右轮位移环PID初始化
                PID_Init(&Xiexian_Angle,-3,-0.2,-13,500,1000); //斜线角度PID初始化
                PID_Init(&Speed_Left,10,2,0.5,500,1000);        //左轮速度环PID初始化
                PID_Init(&Speed_Right,10,2,0.5,500,1000);       //右轮速度环PID初始化
                PID_Init(&Angle,-14,-0.2,-35,500,1000);          //第四问角度环PID初始化, 14,0.2
            }
            
            switch (Curr_State) 
            {
                case 0x00://左上原地转弯
                    if (Last_State==0x07&&Curr_State==0x00) 
                    {
                        Tishi_flag=1;
                    }
                    Last_State=Curr_State;
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -41);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Angle_pid,Right_Angle_pid);
                    if (test_num>=20) 
                    {
                        Curr_State=0x01;
                        test_num=0;
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0x01://左上到右下斜道
                    if (Last_State==0x00&&Curr_State==0x01) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        flag_stop=0;
                        FMQ_flag=1;
                    }
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }
                    Last_State=Curr_State;
                    Left_Distance_pid=Distance_Left_control(&Distance_Left,Encoder_Count_Left,4100);        //距离环
                    Right_Distance_pid=Distance_Right_control(&Distance_Right,Encoder_Count_Right,4100);
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -41);                                   //角度环
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Distance_pid+Left_Angle_pid,Right_Distance_pid+Right_Angle_pid); 
                    if (test_num>=35) 
                    {
                        Curr_State=0x02;
                        test_num=0;
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0x02://右下原地转弯
                    if (Last_State==0x01&&Curr_State==0x02) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, 0);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Angle_pid,Right_Angle_pid);
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }
                    if (test_num>=20) 
                    {
                        Curr_State=0xAA;
                        test_num=0;
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0xAA://右下原地转弯后前走
                    if (!Read_Huidu()) 
                    {
                        set_Motor(300, 350);
                    }
                    else if(Read_Huidu())
                    {
                        Curr_State=0x03;
                        if (FMQ_flag==1) 
                        {
                            Tishi_flag=1;
                        }
                    }
                break;

                case 0x03://右边弯道
                    if ((Last_State==0x02||Last_State==0x01)&&Curr_State==0x03) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        Error_less_flag=1;
                    }
                    Last_State=Curr_State;
                    Gray_Get_Error();                                                       //获取灰度误差
                    Left_speed_pid=Speed_Left_control(&Speed_Left,Left_speed,Speed_goal);           //速度环
                    Right_speed_pid=Speed_Right_control(&Speed_Right,Right_speed,Speed_goal);       //速度环
                    Left_Gray_pid=Gray_control(&Gray,Gray_Error,0);                         //灰度环
                    Right_Gray_pid=-Left_Gray_pid;                                          //灰度环
                    set_Motor(Left_speed_pid+Left_Gray_pid,Right_speed_pid+Right_Gray_pid); //赋值
                    
                    if (!Read_Huidu()) 
                    {
                        
                        Filter_Cnt = 0; // 防止溢出
                        Curr_State = 0xFF;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        Tishi_flag=1;
                        
                    }
                break;

                case 0xFF://右上角出弯延时
                    if (Last_State==0x03&&Curr_State==0xFF) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    set_Motor(0,0);
                    if (test_num>=5) 
                    {
                        Curr_State = 0x04;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0x04://右上角原地转弯
                    if (Last_State==0xFF&&Curr_State==0x04) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -140);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Angle_pid,Right_Angle_pid);
                    if (test_num>=20) 
                    {
                        Curr_State=0x05;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分

                    }
                break;

                case 0x05://右上到左下斜道
                    if (Last_State==0x04&&Curr_State==0x05) 
                    {
                        flag_stop=0;
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        FMQ_flag=1;
                    }
                    Last_State=Curr_State;
                    Left_Distance_pid=Distance_Left_control(&Distance_Left,Encoder_Count_Left,4100);        //距离环
                    Right_Distance_pid=Distance_Right_control(&Distance_Right,Encoder_Count_Right,4100);
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -140);                                   //角度环
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Distance_pid+Left_Angle_pid,Right_Distance_pid+Right_Angle_pid); 
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }
                    if (test_num>=35) 
                    {
                        Curr_State=0x06;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;
                
                case 0x06://左下原地转弯
                    if (Last_State==0x05&&Curr_State==0x06) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -180);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Angle_pid,Right_Angle_pid);
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }
                    if (test_num>=20) 
                    {
                        Curr_State=0xBB;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0xBB://左下原地转弯后前走
                    if (!Read_Huidu()) 
                    {
                        set_Motor(450, 300);
                    }
                    else if(Read_Huidu())
                    {
                        Curr_State=0x07;
                        if (FMQ_flag==1) 
                        {
                            Tishi_flag=1;
                        }
                        
                    }
                break;

                case 0x07://左边弯道
                    if ((Last_State==0x06||Last_State==0x05)&&Curr_State==0x07) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        Error_less_flag=1;          //误差减小
                        flag_Circle=0x06;//用于圈数计算
                    }
                    Last_State=Curr_State;
                    if (flag_Circle==0x06 && Curr_State==0x07) 
                    {
                        flag_Circle=0x07;
                        Circle_Cnt++;  
                    }
                    Gray_Get_Error();                                                       //获取灰度误差
                    Left_speed_pid=Speed_Left_control(&Speed_Left,Left_speed,Speed_goal);           //速度环
                    Right_speed_pid=Speed_Right_control(&Speed_Right,Right_speed,Speed_goal);       //速度环
                    Left_Gray_pid=Gray_control(&Gray,Gray_Error,0);                         //灰度环
                    Right_Gray_pid=-Left_Gray_pid;                                          //灰度环
                    set_Motor(Left_speed_pid+Left_Gray_pid,Right_speed_pid+Right_Gray_pid); //赋值 
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }                   
                    if (!Read_Huidu()&&Circle_Cnt>=4) 
                    {
                        Tishi_flag=1;
                        Curr_State = 0x08;
                    }
                    else if(!Read_Huidu())
                    {
                        Curr_State = 0xFE;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        Tishi_flag=1;
                    }
                break;

                case 0xFE://左上出弯延时
                    if (Last_State==0x07&&Curr_State==0xFE) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    set_Motor(0,0);
                    if (test_num>=5) 
                    {
                        Curr_State = 0x00;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0x08://停车
                    set_Motor(0,0);    
                    Last_State=Curr_State;
                break;

                default:
                    Curr_State=0x00;
                    Last_State=Curr_State;
                break;
            }
        }
///////////////////////////////////////////////////////////////////////////
        else if(cmd=='E')
        {
            if (Last_State == 0x55) 
            {
                PID_Init(&Distance_Left,1,0.001,1.5,500,1000);  //左轮位移环PID初始化
                PID_Init(&Distance_Right,1,0.001,1.5,500,1000); //右轮位移环PID初始化
                PID_Init(&Xiexian_Angle,-3,-0.2,-13,500,1000); //斜线角度PID初始化
                PID_Init(&Speed_Left,10,2,0.5,500,1000);        //左轮速度环PID初始化
                PID_Init(&Speed_Right,10,2,0.5,500,1000);       //右轮速度环PID初始化
                PID_Init(&Angle,-14,-0.2,-35,500,1000);          //第四问角度环PID初始化, 14,0.2
            }
            
            switch (Curr_State) 
            {
                case 0x00://左上原地转弯
                    if (Last_State==0x07&&Curr_State==0x00) 
                    {
                        Tishi_flag=1;
                    }
                    Last_State=Curr_State;
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -41);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Angle_pid,Right_Angle_pid);
                    if (test_num>=20) 
                    {
                        Curr_State=0x01;
                        test_num=0;
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0x01://左上到右下斜道
                    if (Last_State==0x00&&Curr_State==0x01) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        flag_stop=0;
                        FMQ_flag=1;
                    }

                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }

                    Last_State=Curr_State;
                    Left_Distance_pid=Distance_Left_control(&Distance_Left,Encoder_Count_Left,4050);        //距离环
                    Right_Distance_pid=Distance_Right_control(&Distance_Right,Encoder_Count_Right,4050);
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -41);                                   //角度环
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Distance_pid+Left_Angle_pid,Right_Distance_pid+Right_Angle_pid);
                    if (Read_Huidu()) 
                    {
                        Curr_State=0xCC;
                        test_num=0;
                    } 
                    else if (test_num>=35) 
                    {
                        Curr_State=0x02;
                        test_num=0;
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0xCC:
                    set_Motor(200, 800);
                    if (test_num>=10) 
                    {
                        test_num=0;
                        Curr_State=0x03;
                    }
                break;

                case 0x02://右下原地转弯
                    if (Last_State==0x01&&Curr_State==0x02) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, 0);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Angle_pid,Right_Angle_pid);
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }
                    if (test_num>=15) 
                    {
                        Curr_State=0xAA;
                        test_num=0;
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0xAA://右下原地转弯后前走
                    if (!Read_Huidu()) 
                    {
                        set_Motor(300, 450);
                    }
                    else if(Read_Huidu())
                    {
                        Curr_State=0x03;
                        if (FMQ_flag==1) 
                        {
                            Tishi_flag=1;
                        }
                    }
                break;

                case 0x03://右边弯道
                    if ((Last_State==0x02||Last_State==0x01)&&Curr_State==0x03) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        Error_less_flag=1;
                    }
                    Last_State=Curr_State;
                    Gray_Get_Error();                                                       //获取灰度误差
                    Left_speed_pid=Speed_Left_control(&Speed_Left,Left_speed,Speed_goal);           //速度环
                    Right_speed_pid=Speed_Right_control(&Speed_Right,Right_speed,Speed_goal);       //速度环
                    Left_Gray_pid=Gray_control(&Gray,Gray_Error,0);                         //灰度环
                    Right_Gray_pid=-Left_Gray_pid;                                          //灰度环
                    set_Motor(Left_speed_pid+Left_Gray_pid,Right_speed_pid+Right_Gray_pid); //赋值
                    
                    if (!Read_Huidu()) 
                    {
                        
                        Filter_Cnt = 0; // 防止溢出
                        Curr_State = 0x04;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        Tishi_flag=1;
                        
                    }
                break;

                case 0xFF://右上角出弯延时
                    if (Last_State==0x03&&Curr_State==0xFF) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    set_Motor(0,0);
                    if (test_num>=5) 
                    {
                        Curr_State = 0x04;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0x04://右上角原地转弯
                    if (Last_State==0xFF&&Curr_State==0x04) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -141);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Angle_pid,Right_Angle_pid);
                    if (test_num>=20) 
                    {
                        Curr_State=0x05;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分

                    }
                break;

                case 0x05://右上到左下斜道
                    if (Last_State==0x04&&Curr_State==0x05) 
                    {
                        flag_stop=0;
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        FMQ_flag=1;
                    }
                    Last_State=Curr_State;
                    Left_Distance_pid=Distance_Left_control(&Distance_Left,Encoder_Count_Left,4020);        //距离环
                    Right_Distance_pid=Distance_Right_control(&Distance_Right,Encoder_Count_Right,4020);
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -141);                                   //角度环
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Distance_pid+Left_Angle_pid,Right_Distance_pid+Right_Angle_pid); 
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }
                    if (Read_Huidu()) 
                    {
                        Curr_State=0xDD;
                        test_num=0;
                    }
                    else if (test_num>=35) 
                    {
                        Curr_State=0x06;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0xDD:
                    set_Motor(800, 200);
                    if (test_num>=10) 
                    {
                        test_num=0;
                        Curr_State=0x07;
                    }
                break;

                case 0x06://左下原地转弯
                    if (Last_State==0x05&&Curr_State==0x06) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -180);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Angle_pid,Right_Angle_pid);
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }
                    if (test_num>=15) 
                    {
                        Curr_State=0xBB;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0xBB://左下原地转弯后前走
                    if (!Read_Huidu()) 
                    {
                        set_Motor(450, 300);
                    }
                    else if(Read_Huidu())
                    {
                        Curr_State=0x07;
                        if (FMQ_flag==1) 
                        {
                            Tishi_flag=1;
                        }
                        
                    }
                break;

                case 0x07://左边弯道
                    if ((Last_State==0x06||Last_State==0x05)&&Curr_State==0x07) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        Error_less_flag=1;          //误差减小
                        flag_Circle=0x06;//用于圈数计算
                    }
                    Last_State=Curr_State;
                    if (flag_Circle==0x06 && Curr_State==0x07) 
                    {
                        flag_Circle=0x07;
                        Circle_Cnt++;  
                    }
                    Gray_Get_Error();                                                       //获取灰度误差
                    Left_speed_pid=Speed_Left_control(&Speed_Left,Left_speed,Speed_goal);           //速度环
                    Right_speed_pid=Speed_Right_control(&Speed_Right,Right_speed,Speed_goal);       //速度环
                    Left_Gray_pid=Gray_control(&Gray,Gray_Error,0);                         //灰度环
                    Right_Gray_pid=-Left_Gray_pid;                                          //灰度环
                    set_Motor(Left_speed_pid+Left_Gray_pid,Right_speed_pid+Right_Gray_pid); //赋值 
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }                   
                    if (!Read_Huidu()&&Circle_Cnt>=4) 
                    {
                        Tishi_flag=1;
                        Curr_State = 0x08;
                    }
                    else if(!Read_Huidu())
                    {
                        Curr_State = 0x00;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        Tishi_flag=1;
                    }
                break;

                case 0xFE://左上出弯延时
                    if (Last_State==0x07&&Curr_State==0xFE) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    set_Motor(0,0);
                    if (test_num>=5) 
                    {
                        Curr_State = 0x00;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0x08://停车
                    set_Motor(0,0);    
                    Last_State=Curr_State;
                break;

                default:
                    Curr_State=0x00;
                    Last_State=Curr_State;
                break;
            }
        }

        ////////////////////////////////////////////////////////////////
        else if(cmd=='F')
        {
            if (Last_State == 0x55) 
            {
                PID_Init(&Distance_Left,1,0.001,1.5,500,1000);  //左轮位移环PID初始化
                PID_Init(&Distance_Right,1,0.001,1.5,500,1000); //右轮位移环PID初始化
                PID_Init(&Xiexian_Angle,-3,-0.2,-13,500,1000); //斜线角度PID初始化
                PID_Init(&Speed_Left,10,2,0.5,500,1000);        //左轮速度环PID初始化
                PID_Init(&Speed_Right,10,2,0.5,500,1000);       //右轮速度环PID初始化
                PID_Init(&Angle,-14,-0.2,-35,500,1000);          //第四问角度环PID初始化, 14,0.2
            }
            
            switch (Curr_State) 
            {
                case 0x00://左上原地转弯
                    if (Last_State==0x07&&Curr_State==0x00) 
                    {
                        Tishi_flag=1;
                    }
                    Last_State=Curr_State;
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -41);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Angle_pid,Right_Angle_pid);
                    if (test_num>=20) 
                    {
                        Curr_State=0x01;
                        test_num=0;
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0x01://左上到右下斜道
                    if (Last_State==0x00&&Curr_State==0x01) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        flag_stop=0;
                        FMQ_flag=1;
                    }

                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }

                    Last_State=Curr_State;
                    Left_Distance_pid=Distance_Left_control(&Distance_Left,Encoder_Count_Left,4050);        //距离环
                    Right_Distance_pid=Distance_Right_control(&Distance_Right,Encoder_Count_Right,4050);
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -41);                                   //角度环
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Distance_pid+Left_Angle_pid,Right_Distance_pid+Right_Angle_pid);
                    if (Read_Huidu()) 
                    {
                        Curr_State=0xCC;
                        test_num=0;
                    } 
                    else if (test_num>=35) 
                    {
                        Curr_State=0x02;
                        test_num=0;
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0xCC:
                    set_Motor(200, 800);
                    if (test_num>=12) 
                    {
                        test_num=0;
                        Curr_State=0x03;
                    }
                break;

                case 0x02://右下原地转弯
                    if (Last_State==0x01&&Curr_State==0x02) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, 0);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Angle_pid,Right_Angle_pid);
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }
                    if (test_num>=15) 
                    {
                        Curr_State=0xAA;
                        test_num=0;
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0xAA://右下原地转弯后前走
                    if (!Read_Huidu()) 
                    {
                        set_Motor(300, 450);
                    }
                    else if(Read_Huidu())
                    {
                        Curr_State=0x03;
                        if (FMQ_flag==1) 
                        {
                            Tishi_flag=1;
                        }
                    }
                break;

                case 0x03://右边弯道
                    if ((Last_State==0x02||Last_State==0x01)&&Curr_State==0x03) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        Error_less_flag=1;
                    }
                    Last_State=Curr_State;
                    Gray_Get_Error();                                                       //获取灰度误差
                    Left_speed_pid=Speed_Left_control(&Speed_Left,Left_speed,140);           //速度环
                    Right_speed_pid=Speed_Right_control(&Speed_Right,Right_speed,140);       //速度环
                    Left_Gray_pid=Gray_control(&Gray,Gray_Error,0);                         //灰度环
                    Right_Gray_pid=-Left_Gray_pid;                                          //灰度环
                    set_Motor(Left_speed_pid+Left_Gray_pid,Right_speed_pid+Right_Gray_pid); //赋值
                    
                    if (!Read_Huidu()) 
                    {
                        
                        Filter_Cnt = 0; // 防止溢出
                        Curr_State = 0x04;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        Tishi_flag=1;
                        
                    }
                break;

                case 0xFF://右上角出弯延时
                    if (Last_State==0x03&&Curr_State==0xFF) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    set_Motor(0,0);
                    if (test_num>=5) 
                    {
                        Curr_State = 0x04;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0x04://右上角原地转弯
                    if (Last_State==0xFF&&Curr_State==0x04) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -141);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Angle_pid,Right_Angle_pid);
                    if (test_num>=20) 
                    {
                        Curr_State=0x05;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分

                    }
                break;

                case 0x05://右上到左下斜道
                    if (Last_State==0x04&&Curr_State==0x05) 
                    {
                        flag_stop=0;
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        FMQ_flag=1;
                    }
                    Last_State=Curr_State;
                    Left_Distance_pid=Distance_Left_control(&Distance_Left,Encoder_Count_Left,4020);        //距离环
                    Right_Distance_pid=Distance_Right_control(&Distance_Right,Encoder_Count_Right,4020);
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -141);                                   //角度环
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Distance_pid+Left_Angle_pid,Right_Distance_pid+Right_Angle_pid); 
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }
                    if (Read_Huidu()) 
                    {
                        Curr_State=0xDD;
                        test_num=0;
                    }
                    else if (test_num>=35) 
                    {
                        Curr_State=0x06;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0xDD:
                    set_Motor(800, 200);
                    if (test_num>=12) 
                    {
                        test_num=0;
                        Curr_State=0x07;
                    }
                break;

                case 0x06://左下原地转弯
                    if (Last_State==0x05&&Curr_State==0x06) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    Left_Angle_pid=Angle_control(&Angle, yaw_angle, -180);
                    Right_Angle_pid=-Left_Angle_pid;
                    set_Motor(Left_Angle_pid,Right_Angle_pid);
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }
                    if (test_num>=15) 
                    {
                        Curr_State=0xBB;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0xBB://左下原地转弯后前走
                    if (!Read_Huidu()) 
                    {
                        set_Motor(450, 300);
                    }
                    else if(Read_Huidu())
                    {
                        Curr_State=0x07;
                        if (FMQ_flag==1) 
                        {
                            Tishi_flag=1;
                        }
                        
                    }
                break;

                case 0x07://左边弯道
                    if ((Last_State==0x06||Last_State==0x05)&&Curr_State==0x07) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        Error_less_flag=1;          //误差减小
                        flag_Circle=0x06;//用于圈数计算
                    }
                    Last_State=Curr_State;
                    if (flag_Circle==0x06 && Curr_State==0x07) 
                    {
                        flag_Circle=0x07;
                        Circle_Cnt++;  
                    }
                    Gray_Get_Error();                                                       //获取灰度误差
                    Left_speed_pid=Speed_Left_control(&Speed_Left,Left_speed,140);           //速度环
                    Right_speed_pid=Speed_Right_control(&Speed_Right,Right_speed,140);       //速度环
                    Left_Gray_pid=Gray_control(&Gray,Gray_Error,0);                         //灰度环
                    Right_Gray_pid=-Left_Gray_pid;                                          //灰度环
                    set_Motor(Left_speed_pid+Left_Gray_pid,Right_speed_pid+Right_Gray_pid); //赋值 
                    if (Read_Huidu()&&FMQ_flag==1) 
                    {
                        FMQ_flag=0;
                        Tishi_flag=1;
                    }                   
                    if (!Read_Huidu()&&Circle_Cnt>=4) 
                    {
                        Tishi_flag=1;
                        Curr_State = 0x08;
                    }
                    else if(!Read_Huidu())
                    {
                        Curr_State = 0x00;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                        Tishi_flag=1;
                    }
                break;

                case 0xFE://左上出弯延时
                    if (Last_State==0x07&&Curr_State==0xFE) 
                    {
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                    Last_State=Curr_State;
                    set_Motor(0,0);
                    if (test_num>=5) 
                    {
                        Curr_State = 0x00;
                        test_num=0;         //延时清零
                        set_Motor(0, 0);
                        Encoder_Count_Left=0;       //位移值清零
                        Encoder_Count_Right=0;      //位移值清零
                        Distance_Left.integral=0;   //清位移环积分
                        Distance_Right.integral=0;  //清位移环积分
                        Angle.integral=0;           //清积分
                        Xiexian_Angle.integral=0;   //清积分
                    }
                break;

                case 0x08://停车
                    set_Motor(0,0);    
                    Last_State=Curr_State;
                break;

                default:
                    Curr_State=0x00;
                    Last_State=Curr_State;
                break;
            }
        }
    }
}